#include "HandEyeCal.h"
#include <opencv2\calib3d.hpp>
#include <pcl\registration\transformation_estimation_svd.h>
//bool
//HandEyeCal::eyeToHand(std::vector<LocationDH>& input_camera, std::vector<LocationDH>& input_robot, Eigen::Matrix4f& output_mat)
//{
//	std::vector<cv::Point3f> first, second;
//	for (int i = 0; i < input_camera.size(); ++i)
//	{
//		LocationDH& cam = input_camera[i];
//		LocationDH& rob = input_robot[i];
//		first.push_back(cv::Point3f(cam.x, cam.y, cam.z));
//		second.push_back(cv::Point3f(rob.x, rob.y, rob.z));
//	}
//	cv::Mat aff(4, 3, CV_64FC1);
//	std::vector<unsigned char> inliers;
//	cv::estimateAffine3D(first, second, aff, inliers,4,0.5);
//	output_mat.setIdentity();
//	for (int i = 0; i < 3; ++i)
//		for (int j = 0; j < 4; ++j)
//			output_mat(i, j) = aff.at<double>(i, j);
//	return true;
//}
bool
HandEyeCal::eyeToHand(std::vector<LocationDH>& input_camera, std::vector<LocationDH>& input_robot, Eigen::Matrix4f& output_mat)
{
	pcl::PointCloud<pcl::PointXYZ> first, second;
	for (int i = 0; i < input_camera.size(); ++i)
	{
		LocationDH& cam = input_camera[i];
		LocationDH& rob = input_robot[i];
		first.push_back(pcl::PointXYZ(cam.x, cam.y, cam.z));
		second.push_back(pcl::PointXYZ(rob.x, rob.y, rob.z));
	}
	pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> te;
	te.estimateRigidTransformation(first, second, output_mat);
	return true;
}